#include <arch/philips/lpc2103.h>
#include "fixed.h"
#include "sys.h"
#include "util.h"
#include "car.h" 

static void servo_ISR() __attribute__ ((interrupt ("IRQ")));

static servo_func servoCallback;
static int servoReady = 0;
static fixedp servoPos = 0;			// Q4.28
static fixedp motorSpeed = 0;		// Q4.28
static motor_mode motorMode = 0;

// note that we are generating INVERTING PWM since the signal is
// inverted electrically before being sent to the servo
void servo_init(void)
{
	servoCallback = 0;
	servoReady = 0;
	servoPos = 0;

	T3TCR = 2;						// put timer3 in reset
	PINSEL1 |= (0x02 << 28);		// select MAT3.3 for P0.30 pin function
	T3CTCR = 0;						// select timer mode
	T3MCR = (1 << 0) | (1 << 1);	// reset and interrupt on match0
	T3PWMCON = (1 << 3);			// enable PWM mode for MAT3.3
	T3PR = 14;						// select a prescale of 15 so that we have a resolution of 1us
									// Changed to 15 as PCLK is 60/4 = 15MHz. Gives 1us resolution - william
	T3MR0 = SERVO_PERIOD;			// set fundamental frequency of 50Hz (20ms between pulses)
	T3MR3 = SERVO_CENTER;			// set the initial servo pulse width
	
	VICVectAddr2 = (unsigned long)servo_ISR;	// set interrupt vector address [man52]
	VICVectCntl2 = 27 | (1 << 5);		// interrupt source with index 27 (timer3) is enabled
										// as the one with priority 2. man[51], man[58]
	VICIntEnable |= (1 << 27);			// enable interrupt for timer3 [man47]
	
	T3TCR = 1;						// take timer3 out of reset and enable for counting
}

int servo_ready(void)
{
	return servoReady;
}

// the PWM value must be set synchronously, so here we will just store
// the value until the next interrupt is fired
void servo_set(fixedp pos)
{
	servoPos = clampq(pos, int2q(-1), int2q(1));
	servoReady = 0;
}

void servo_set_callback(servo_func func)
{
	servoCallback = func;
}

fixedp servo_get(void)
{
	return servoPos;
}

// this interrupt is fired at the beginning of the PWM cycle (the signal
// is brought LOW, which means the signal at the servo is HIGH)
void servo_ISR()
{
	servoReady = 1;
	if(servoCallback)
		servoCallback();
	//uart0_puts("Servo position:\n");
	//uart0_puts(int2str(val));
	T3MR3 = SERVO_CENTER + q2int(qmul(servoPos, int2q(SERVO_SCALE)));
	T3IR = 0xff;					// clear all interrupts by writing ones
	VICVectAddr = 0;
}

// note that we are generating NONINVERTING PWM, and we are not bothering
// to update the PWM cycle length synchronously as in the servo module
// because the frequency is much higher.
void motor_init(void)
{
	T1TCR = 2;						// put timer1 in reset
	PINSEL0 |= 2 << 26;				// select MAT1.1 for P0.13 pin function
	PINSEL1 |= 2 << 6;				// select MAT1.2 for P0.19 pin function
	T1CTCR = 0;						// select timer mode
	T1MCR = (1 << 10);				// reset on match3
	T1PWMCON = (1 << 1) | (1 << 2);	// enable PWM mode for MAT1.1 (SPD) and MAT1.2 (BRK)
	T1PR = 14;						// Resolution of 1us
	T1MR3 = MOTOR_PERIOD;			// set the PWM cycle length
	T1MR1 = MOTOR_PERIOD;			// motor OFF
	T1MR2 = MOTOR_PERIOD;			// brake OFF
	
	T1TCR = 1;						// take timer1 out of reset and enable for counting
}

void motor_set(fixedp speed, motor_mode mode)
{
	motorSpeed = clampq(speed, 0, float2q(1.0));
	motorMode = mode;
	
	// use less fracbits for the multiplication with MOTOR_PERIOD because it is a large number
	// pw = MOTOR_PERIOD * (1.0 - MOTOR_MAX * motorSpeed);
	int pw = _q2int(_qmul(_int2q(MOTOR_PERIOD, 4), qsub(float2q(1.0), qmul(float2q(MOTOR_MAX), motorSpeed)), 4, FIXED_FRACBITS, 4), 4);
	
	switch(mode) {
	case brake:
		T1MR1 = MOTOR_PERIOD; 			// no gas
		T1MR2 = pw;
		break;
	case accel:
		T1MR2 = MOTOR_PERIOD;			// no brakes
		T1MR1 = pw;
		break;
	case coast:
	default:
		T1MR1 = MOTOR_PERIOD;			// brake off
		T1MR2 = MOTOR_PERIOD;			// motor off
	}
}

void motor_get(fixedp *speed, motor_mode *mode)
{
	*speed = motorSpeed;
	*mode = motorMode;
}

